#include "SDFSprayAlgorithm.h"
#include <iostream>
#include <osg/Geometry>
#include <osg/LineWidth>

SDFSprayAlgorithm* SDFSprayAlgorithm::_instance = NULL;

SDFSprayAlgorithm::SDFSprayAlgorithm()
{
    m_pointsGeode = new osg::Geode;
    m_pointsMT = new osg::MatrixTransform;
    m_pointsMT->addChild(m_pointsGeode);

    m_radius = 0.02;
}

SDFSprayAlgorithm::~SDFSprayAlgorithm()
{

}

void SDFSprayAlgorithm::generatePoints(osg::ref_ptr<SDF> sdfPtr)
{
    //std::cout<<"SDFSprayAlgorithm::generatePoints sdfPtr="<<(int)(sdfPtr.get())<<std::endl;

    if(sdfPtr.get() == NULL)
    {
        return;
    }

    m_points.clear();
    m_normals.clear();

    m_pointsMT->setMatrix( sdfPtr->getMatrix() );

    int countX = sdfPtr->getNodeCountVector().x();
    int countY = sdfPtr->getNodeCountVector().y();
    int countZ = sdfPtr->getNodeCountVector().z();
    float dx = sdfPtr->getdxdydz().x();
    float dy = sdfPtr->getdxdydz().y();
    float dz = sdfPtr->getdxdydz().z();
    int di = m_radius / dx;
    int dj = m_radius / dy;
    int dk = m_radius / dz;
    std::cout<<"SDFSprayAlgorithm::generatePoints count="<<countX<<","<<countY<<","<<countZ<<std::endl;

    int start_i, start_j, start_k;
    for(int i = 3; i < 4; i = i+1)//int i = 0; i < countX; i++
    {
        std::vector<osg::Vec3> thePointsJK;
        for(int j = 0; j < countY; j=j+1)
        {
            for(int k = 0; k < countZ; k=k+1)
            {
                double gridValue = sdfPtr->getGridValueByIJK(i,j,k);
                if(gridValue > 0 && sdfPtr->isPointClosedByIJK4(i,j,k))
                {
                    start_i = i;
                    start_j = j;
                    start_k = k;
                    osg::Vec3 position = sdfPtr->getPositionInSDFFrameByIJK(i,j,k);
                    osg::Vec3 gradient = sdfPtr->getGradientByIJK(i,j,k);

                    thePointsJK.push_back(position);
                    m_normals.push_back(gradient);
                    m_indexes.push_back(osg::Vec3(i,j,k));
                }
            }
        }
        repairTrajectory(thePointsJK);
        for(int j = 0 ; j < thePointsJK.size(); j++)
        {
            m_points.push_back(thePointsJK[j]);
        }
    }
    updatePointsGeode();
}

void SDFSprayAlgorithm::writePointsToFile(std::string fileName)
{

}

std::vector<osg::Vec3> SDFSprayAlgorithm::getPoints()
{
    return m_points;
}

std::vector<osg::Vec3> SDFSprayAlgorithm::getNormals()
{
    return m_normals;
}

osg::ref_ptr<osg::Geode> SDFSprayAlgorithm::getPointsGeode()
{
    return m_pointsGeode;
}

osg::ref_ptr<osg::MatrixTransform> SDFSprayAlgorithm::getPointsMT()
{
    return m_pointsMT;
}

void SDFSprayAlgorithm::repairTrajectory(std::vector<osg::Vec3>& points)
{
    if(points.size() == 0)
    {
        return;
    }

    std::vector<osg::Vec3> trajectory = points;
    std::vector<osg::Vec3>& normals = m_normals;
    std::vector<osg::Vec3>& indexes = m_indexes;
    std::vector<osg::Vec3> newTrajectory;
    int i = 0;
    //int nearestIndex = 1;
    newTrajectory.push_back(trajectory[0]);
    //trajectory.erase(trajectory.begin());
    //trajectory[0] = osg::Vec3(999,999,999);
    while( newTrajectory.size() < trajectory.size() )//nearestIndex != i
    {
        int nearestIndex = findNearestPoint(i, trajectory);
        newTrajectory.push_back(trajectory[nearestIndex]);
        trajectory[i] = osg::Vec3(999,999,999);
        //std::vector<osg::Vec3>::iterator  findedIt = trajectory.begin() + i;
        //trajectory.erase(findedIt);

        i = nearestIndex;
    }
    points = newTrajectory;
}

void SDFSprayAlgorithm::repairTrajectory2(std::vector<osg::Vec3> &points)
{
    if(points.size() == 0)
    {
        return;
    }
    std::vector<osg::Vec3> trajectory = points;
    std::vector<int> usedIndex;
    for(int i = 0 ; i < trajectory.size(); i++)
    {
        if(trajectory[i].length() < 10)
        {
            int nearestIndex = findNearestPoint(i, trajectory);
            trajectory[i] = osg::Vec3(11,11,11);
        }
    }
}

void SDFSprayAlgorithm::updatePointsGeode()
{
    m_pointsGeode->removeDrawables(0, m_pointsGeode->getNumDrawables());

    osg::ref_ptr<osg::Vec3Array> m_pointVector;
    osg::ref_ptr<osg::Geometry> m_geom;
    osg::ref_ptr<osg::LineWidth> m_lineWidth;
    osg::ref_ptr<osg::Vec4Array> m_colorArray;
    osg::ref_ptr<osg::Vec3Array> normalArray;

    //add coordinate
    m_geom = new osg::Geometry;
    m_pointVector = new osg::Vec3Array;
    m_colorArray = new osg::Vec4Array;
    normalArray = new osg::Vec3Array;


    //color:
    m_colorArray->push_back(osg::Vec4(1,0,0,1));
    m_geom->setColorArray( m_colorArray.get() );
    m_geom->setColorBinding( osg::Geometry::BIND_OVERALL );

    //vertex
    m_geom->setVertexArray( m_pointVector.get() );

    //normal
    m_geom->setNormalArray( normalArray.get() );
    m_geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );

    //
    for(int i = 0; i < m_points.size(); i++)
    {
        m_pointVector->push_back(m_points[i]);
        if(i < m_normals.size())
        {
            normalArray->push_back(m_normals[i]);
        }
    }

    m_geom->addPrimitiveSet(new osg::DrawArrays( osg::PrimitiveSet::LINE_STRIP, 0,  m_pointVector->size() ) );

    //line-size
    m_lineWidth = new osg::LineWidth;
    m_lineWidth ->setWidth(2);
    m_geom->getOrCreateStateSet()->setAttributeAndModes(m_lineWidth.get (),osg::StateAttribute::ON);

    m_pointsGeode->addDrawable(m_geom);

    osg::StateSet* stateset = m_pointsGeode->getOrCreateStateSet();
    stateset->setMode(GL_LIGHTING, osg::StateAttribute::OFF);//这里GL_LIGHTING是系统opengl的头文件GL.h中定义的

}

int SDFSprayAlgorithm::findNearestPoint(int pointIndex, std::vector<osg::Vec3> &points)
{
    double minDistance = 999999;
    int resultIndex = 0;
    for(int i = 0; i < points.size(); i++)
    {
        if( i!= pointIndex )
        {
            double distance = (points[i] - points[pointIndex]).length();
            if( minDistance >  distance)
            {
                resultIndex = i;
                minDistance = distance;
            }
        }
    }
    return resultIndex;
}

SDFSprayAlgorithm *SDFSprayAlgorithm::instance()
{
    if (_instance == NULL)
    {
        _instance = new SDFSprayAlgorithm();
    }
    return _instance;
}
